<!-- saved from url=(0014)about:internet -->
<html>
<head>
<meta http-equiv="content-type" content="text/html; charset=utf-8" />
<title>RTW Report - nav_h_ekf.c</title>
<script language="JavaScript" type="text/javascript" src="rtwreport_utils.js"></script>
</head>
<body bgcolor="#eeeeee" text="#1122aa" onload="try {if (top) { if (top.rtwFileOnLoad) top.rtwFileOnLoad(window.document); else local_onload();}} catch(err) {};">
<p>
<table border="0" cellspacing="0" cellpadding="6" width="100%%" height="100%%"><tr><td width="100%%" valign="top" bgcolor="#ffffff">
<h4>File: <a href="../nav_h_ekf.c" target="rtwreport_document_frame" id="linkToText_plain">nav_h_ekf.c</a></h4>
<pre id="RTWcode">
<a name="1"><font color="#888888 "><i>    1   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="2"><font color="#888888 "><i>    2   </i></font></a><font color="#117755"><I> * File: nav_h_ekf.c</i></font>
<a name="3"><font color="#888888 "><i>    3   </i></font></a><font color="#117755"><I> *</i></font>
<a name="4"><font color="#888888 "><i>    4   </i></font></a><font color="#117755"><I> * MATLAB Coder version            : 2.6</i></font>
<a name="5"><font color="#888888 "><i>    5   </i></font></a><font color="#117755"><I> * C/C++ source code generated on  : 03-Jul-2017 11:01:12</i></font>
<a name="6"><font color="#888888 "><i>    6   </i></font></a><font color="#117755"><I> */</i></font>
<a name="7"><font color="#888888 "><i>    7   </i></font></a>
<a name="8"><font color="#888888 "><i>    8   </i></font></a><font color="#117755"><i>/* Include files */</i></font>
<a name="9"><font color="#888888 "><i>    9   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;rt_nonfinite.h&quot;</font>
<a name="10"><font color="#888888 "><i>   10   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;nav_h_ekf.h&quot;</font>
<a name="11"><font color="#888888 "><i>   11   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;svd.h&quot;</font>
<a name="12"><font color="#888888 "><i>   12   </i></font></a>
<a name="13"><font color="#888888 "><i>   13   </i></font></a><font color="#117755"><i>/* Variable Definitions */</i></font>
<a name="14"><font color="#888888 "><i>   14   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_X">X</a>[3];
<a name="15"><font color="#888888 "><i>   15   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_P">P</a>[9];
<a name="16"><font color="#888888 "><i>   16   </i></font></a>
<a name="17"><font color="#888888 "><i>   17   </i></font></a><font color="#117755"><i>/* Function Declarations */</i></font>
<a name="18"><font color="#888888 "><i>   18   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_b_eml_xscal" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_xscal');" target="_self"><font color="#1122aa">b_eml_xscal</font></a>(<font color="#112266"><b>float</b></font> a, <font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>int</b></font> ix0);
<a name="19"><font color="#888888 "><i>   19   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a href="#fcn_c_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_c_eml_div');" target="_self"><font color="#1122aa">c_eml_div</font></a>(<font color="#112266"><b>float</b></font> x, <font color="#112266"><b>double</b></font> y);
<a name="20"><font color="#888888 "><i>   20   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a href="#fcn_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_div');" target="_self"><font color="#1122aa">eml_div</font></a>(<font color="#112266"><b>double</b></font> x, <font color="#112266"><b>float</b></font> y);
<a name="21"><font color="#888888 "><i>   21   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_eml_xaxpy" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xaxpy');" target="_self"><font color="#1122aa">eml_xaxpy</font></a>(<font color="#112266"><b>float</b></font> a, <font color="#112266"><b>float</b></font> y[4]);
<a name="22"><font color="#888888 "><i>   22   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a href="#fcn_eml_xdotc" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xdotc');" target="_self"><font color="#1122aa">eml_xdotc</font></a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> y[4]);
<a name="23"><font color="#888888 "><i>   23   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_eml_xgesvd" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xgesvd');" target="_self"><font color="#1122aa">eml_xgesvd</font></a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> A[4], <font color="#112266"><b>float</b></font> U[4], <font color="#112266"><b>float</b></font> S[2], <font color="#112266"><b>float</b></font> V[4]);
<a name="24"><font color="#888888 "><i>   24   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a href="#fcn_eml_xnrm2" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xnrm2');" target="_self"><font color="#1122aa">eml_xnrm2</font></a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[4]);
<a name="25"><font color="#888888 "><i>   25   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_eml_xrot" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrot');" target="_self"><font color="#1122aa">eml_xrot</font></a>(<font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>int</b></font> ix0, <font color="#112266"><b>int</b></font> iy0, <font color="#112266"><b>float</b></font> c, <font color="#112266"><b>float</b></font> s);
<a name="26"><font color="#888888 "><i>   26   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_eml_xrotg" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrotg');" target="_self"><font color="#1122aa">eml_xrotg</font></a>(<font color="#112266"><b>float</b></font> *a, <font color="#112266"><b>float</b></font> *b, <font color="#112266"><b>float</b></font> *c, <font color="#112266"><b>float</b></font> *s);
<a name="27"><font color="#888888 "><i>   27   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_eml_xscal" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xscal');" target="_self"><font color="#1122aa">eml_xscal</font></a>(<font color="#112266"><b>float</b></font> a, <font color="#112266"><b>float</b></font> x[4]);
<a name="28"><font color="#888888 "><i>   28   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_eml_xswap" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xswap');" target="_self"><font color="#1122aa">eml_xswap</font></a>(<font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>int</b></font> ix0, <font color="#112266"><b>int</b></font> iy0);
<a name="29"><font color="#888888 "><i>   29   </i></font></a>
<a name="30"><font color="#888888 "><i>   30   </i></font></a><font color="#117755"><i>/* Function Definitions */</i></font>
<a name="31"><font color="#888888 "><i>   31   </i></font></a>
<a name="32"><font color="#888888 "><i>   32   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="33"><font color="#888888 "><i>   33   </i></font></a><font color="#117755"><I> * Arguments    : float a</i></font>
<a name="34"><font color="#888888 "><i>   34   </i></font></a><font color="#117755"><I> *                float x[4]</i></font>
<a name="35"><font color="#888888 "><i>   35   </i></font></a><font color="#117755"><I> *                int ix0</i></font>
<a name="36"><font color="#888888 "><i>   36   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="37"><font color="#888888 "><i>   37   </i></font></a><font color="#117755"><I> */</i></font>
<a name="38"><font color="#888888 "><i>   38   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_b_eml_xscal">b_eml_xscal</a>(<font color="#112266"><b>float</b></font> a, <font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>int</b></font> ix0)
<a name="39"><font color="#888888 "><i>   39   </i></font></a><b>{</b>
<a name="40"><font color="#888888 "><i>   40   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="41"><font color="#888888 "><i>   41   </i></font></a>  <font color="#112266"><b>for</b></font> (k = ix0; k &lt;= ix0 + 1; k++) <b>{</b>
<a name="42"><font color="#888888 "><i>   42   </i></font></a>    x[k - 1] *= a;
<a name="43"><font color="#888888 "><i>   43   </i></font></a>  <b>}</b>
<a name="44"><font color="#888888 "><i>   44   </i></font></a><b>}</b>
<a name="45"><font color="#888888 "><i>   45   </i></font></a>
<a name="46"><font color="#888888 "><i>   46   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="47"><font color="#888888 "><i>   47   </i></font></a><font color="#117755"><I> * Arguments    : float x</i></font>
<a name="48"><font color="#888888 "><i>   48   </i></font></a><font color="#117755"><I> *                double y</i></font>
<a name="49"><font color="#888888 "><i>   49   </i></font></a><font color="#117755"><I> * Return Type  : float</i></font>
<a name="50"><font color="#888888 "><i>   50   </i></font></a><font color="#117755"><I> */</i></font>
<a name="51"><font color="#888888 "><i>   51   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="fcn_c_eml_div">c_eml_div</a>(<font color="#112266"><b>float</b></font> x, <font color="#112266"><b>double</b></font> y)
<a name="52"><font color="#888888 "><i>   52   </i></font></a><b>{</b>
<a name="53"><font color="#888888 "><i>   53   </i></font></a>  <font color="#112266"><b>return</b></font> x / (<font color="#112266"><b>float</b></font>)y;
<a name="54"><font color="#888888 "><i>   54   </i></font></a><b>}</b>
<a name="55"><font color="#888888 "><i>   55   </i></font></a>
<a name="56"><font color="#888888 "><i>   56   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="57"><font color="#888888 "><i>   57   </i></font></a><font color="#117755"><I> * Arguments    : double x</i></font>
<a name="58"><font color="#888888 "><i>   58   </i></font></a><font color="#117755"><I> *                float y</i></font>
<a name="59"><font color="#888888 "><i>   59   </i></font></a><font color="#117755"><I> * Return Type  : float</i></font>
<a name="60"><font color="#888888 "><i>   60   </i></font></a><font color="#117755"><I> */</i></font>
<a name="61"><font color="#888888 "><i>   61   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="fcn_eml_div">eml_div</a>(<font color="#112266"><b>double</b></font> x, <font color="#112266"><b>float</b></font> y)
<a name="62"><font color="#888888 "><i>   62   </i></font></a><b>{</b>
<a name="63"><font color="#888888 "><i>   63   </i></font></a>  <font color="#112266"><b>return</b></font> (<font color="#112266"><b>float</b></font>)x / y;
<a name="64"><font color="#888888 "><i>   64   </i></font></a><b>}</b>
<a name="65"><font color="#888888 "><i>   65   </i></font></a>
<a name="66"><font color="#888888 "><i>   66   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="67"><font color="#888888 "><i>   67   </i></font></a><font color="#117755"><I> * Arguments    : float a</i></font>
<a name="68"><font color="#888888 "><i>   68   </i></font></a><font color="#117755"><I> *                float y[4]</i></font>
<a name="69"><font color="#888888 "><i>   69   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="70"><font color="#888888 "><i>   70   </i></font></a><font color="#117755"><I> */</i></font>
<a name="71"><font color="#888888 "><i>   71   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_eml_xaxpy">eml_xaxpy</a>(<font color="#112266"><b>float</b></font> a, <font color="#112266"><b>float</b></font> y[4])
<a name="72"><font color="#888888 "><i>   72   </i></font></a><b>{</b>
<a name="73"><font color="#888888 "><i>   73   </i></font></a>  <font color="#112266"><b>int</b></font> ix;
<a name="74"><font color="#888888 "><i>   74   </i></font></a>  <font color="#112266"><b>int</b></font> iy;
<a name="75"><font color="#888888 "><i>   75   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="76"><font color="#888888 "><i>   76   </i></font></a>  <font color="#112266"><b>if</b></font> (a == 0.0F) <b>{</b>
<a name="77"><font color="#888888 "><i>   77   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="78"><font color="#888888 "><i>   78   </i></font></a>    ix = 0;
<a name="79"><font color="#888888 "><i>   79   </i></font></a>    iy = 2;
<a name="80"><font color="#888888 "><i>   80   </i></font></a>    <font color="#112266"><b>for</b></font> (k = 0; k &lt; 2; k++) <b>{</b>
<a name="81"><font color="#888888 "><i>   81   </i></font></a>      y[iy] += a * y[ix];
<a name="82"><font color="#888888 "><i>   82   </i></font></a>      ix++;
<a name="83"><font color="#888888 "><i>   83   </i></font></a>      iy++;
<a name="84"><font color="#888888 "><i>   84   </i></font></a>    <b>}</b>
<a name="85"><font color="#888888 "><i>   85   </i></font></a>  <b>}</b>
<a name="86"><font color="#888888 "><i>   86   </i></font></a><b>}</b>
<a name="87"><font color="#888888 "><i>   87   </i></font></a>
<a name="88"><font color="#888888 "><i>   88   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="89"><font color="#888888 "><i>   89   </i></font></a><font color="#117755"><I> * Arguments    : const float x[4]</i></font>
<a name="90"><font color="#888888 "><i>   90   </i></font></a><font color="#117755"><I> *                const float y[4]</i></font>
<a name="91"><font color="#888888 "><i>   91   </i></font></a><font color="#117755"><I> * Return Type  : float</i></font>
<a name="92"><font color="#888888 "><i>   92   </i></font></a><font color="#117755"><I> */</i></font>
<a name="93"><font color="#888888 "><i>   93   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="fcn_eml_xdotc">eml_xdotc</a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> y[4])
<a name="94"><font color="#888888 "><i>   94   </i></font></a><b>{</b>
<a name="95"><font color="#888888 "><i>   95   </i></font></a>  <font color="#112266"><b>float</b></font> d;
<a name="96"><font color="#888888 "><i>   96   </i></font></a>  <font color="#112266"><b>int</b></font> ix;
<a name="97"><font color="#888888 "><i>   97   </i></font></a>  <font color="#112266"><b>int</b></font> iy;
<a name="98"><font color="#888888 "><i>   98   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="99"><font color="#888888 "><i>   99   </i></font></a>  d = 0.0F;
<a name="100"><font color="#888888 "><i>  100   </i></font></a>  ix = 0;
<a name="101"><font color="#888888 "><i>  101   </i></font></a>  iy = 2;
<a name="102"><font color="#888888 "><i>  102   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 2; k++) <b>{</b>
<a name="103"><font color="#888888 "><i>  103   </i></font></a>    d += x[ix] * y[iy];
<a name="104"><font color="#888888 "><i>  104   </i></font></a>    ix++;
<a name="105"><font color="#888888 "><i>  105   </i></font></a>    iy++;
<a name="106"><font color="#888888 "><i>  106   </i></font></a>  <b>}</b>
<a name="107"><font color="#888888 "><i>  107   </i></font></a>
<a name="108"><font color="#888888 "><i>  108   </i></font></a>  <font color="#112266"><b>return</b></font> d;
<a name="109"><font color="#888888 "><i>  109   </i></font></a><b>}</b>
<a name="110"><font color="#888888 "><i>  110   </i></font></a>
<a name="111"><font color="#888888 "><i>  111   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="112"><font color="#888888 "><i>  112   </i></font></a><font color="#117755"><I> * Arguments    : const float A[4]</i></font>
<a name="113"><font color="#888888 "><i>  113   </i></font></a><font color="#117755"><I> *                float U[4]</i></font>
<a name="114"><font color="#888888 "><i>  114   </i></font></a><font color="#117755"><I> *                float S[2]</i></font>
<a name="115"><font color="#888888 "><i>  115   </i></font></a><font color="#117755"><I> *                float V[4]</i></font>
<a name="116"><font color="#888888 "><i>  116   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="117"><font color="#888888 "><i>  117   </i></font></a><font color="#117755"><I> */</i></font>
<a name="118"><font color="#888888 "><i>  118   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_eml_xgesvd">eml_xgesvd</a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> A[4], <font color="#112266"><b>float</b></font> U[4], <font color="#112266"><b>float</b></font> S[2], <font color="#112266"><b>float</b></font> V[4])
<a name="119"><font color="#888888 "><i>  119   </i></font></a><b>{</b>
<a name="120"><font color="#888888 "><i>  120   </i></font></a>  <font color="#112266"><b>float</b></font> b_A[4];
<a name="121"><font color="#888888 "><i>  121   </i></font></a>  <font color="#112266"><b>float</b></font> Vf[4];
<a name="122"><font color="#888888 "><i>  122   </i></font></a>  <font color="#112266"><b>int</b></font> kase;
<a name="123"><font color="#888888 "><i>  123   </i></font></a>  <font color="#112266"><b>float</b></font> nrm;
<a name="124"><font color="#888888 "><i>  124   </i></font></a>  <font color="#112266"><b>float</b></font> s[2];
<a name="125"><font color="#888888 "><i>  125   </i></font></a>  <font color="#112266"><b>int</b></font> m;
<a name="126"><font color="#888888 "><i>  126   </i></font></a>  <font color="#112266"><b>float</b></font> e[2];
<a name="127"><font color="#888888 "><i>  127   </i></font></a>  <font color="#112266"><b>int</b></font> q;
<a name="128"><font color="#888888 "><i>  128   </i></font></a>  <font color="#112266"><b>float</b></font> rt;
<a name="129"><font color="#888888 "><i>  129   </i></font></a>  <font color="#112266"><b>float</b></font> ztest;
<a name="130"><font color="#888888 "><i>  130   </i></font></a>  <font color="#112266"><b>int</b></font> iter;
<a name="131"><font color="#888888 "><i>  131   </i></font></a>  <font color="#112266"><b>float</b></font> tiny;
<a name="132"><font color="#888888 "><i>  132   </i></font></a>  <font color="#112266"><b>float</b></font> snorm;
<a name="133"><font color="#888888 "><i>  133   </i></font></a>  <a href="rtwtypes_h.html#type_boolean_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_boolean_T');" target="_self"><font color="#1122aa">boolean_T</font></a> exitg3;
<a name="134"><font color="#888888 "><i>  134   </i></font></a>  <font color="#112266"><b>int</b></font> qs;
<a name="135"><font color="#888888 "><i>  135   </i></font></a>  <a href="rtwtypes_h.html#type_boolean_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_boolean_T');" target="_self"><font color="#1122aa">boolean_T</font></a> exitg2;
<a name="136"><font color="#888888 "><i>  136   </i></font></a>  <font color="#112266"><b>float</b></font> f;
<a name="137"><font color="#888888 "><i>  137   </i></font></a>  <font color="#112266"><b>float</b></font> varargin_1[5];
<a name="138"><font color="#888888 "><i>  138   </i></font></a>  <font color="#112266"><b>float</b></font> mtmp;
<a name="139"><font color="#888888 "><i>  139   </i></font></a>  <a href="rtwtypes_h.html#type_boolean_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_boolean_T');" target="_self"><font color="#1122aa">boolean_T</font></a> exitg1;
<a name="140"><font color="#888888 "><i>  140   </i></font></a>  <font color="#112266"><b>float</b></font> sqds;
<a name="141"><font color="#888888 "><i>  141   </i></font></a>  <font color="#112266"><b>for</b></font> (kase = 0; kase &lt; 4; kase++) <b>{</b>
<a name="142"><font color="#888888 "><i>  142   </i></font></a>    b_A[kase] = A[kase];
<a name="143"><font color="#888888 "><i>  143   </i></font></a>    Vf[kase] = 0.0F;
<a name="144"><font color="#888888 "><i>  144   </i></font></a>  <b>}</b>
<a name="145"><font color="#888888 "><i>  145   </i></font></a>
<a name="146"><font color="#888888 "><i>  146   </i></font></a>  nrm = <a href="#fcn_eml_xnrm2" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xnrm2');" target="_self"><font color="#1122aa">eml_xnrm2</font></a>(A);
<a name="147"><font color="#888888 "><i>  147   </i></font></a>  <font color="#112266"><b>if</b></font> (nrm &gt; 0.0F) <b>{</b>
<a name="148"><font color="#888888 "><i>  148   </i></font></a>    <font color="#112266"><b>if</b></font> (A[0] &lt; 0.0F) <b>{</b>
<a name="149"><font color="#888888 "><i>  149   </i></font></a>      nrm = -nrm;
<a name="150"><font color="#888888 "><i>  150   </i></font></a>    <b>}</b>
<a name="151"><font color="#888888 "><i>  151   </i></font></a>
<a name="152"><font color="#888888 "><i>  152   </i></font></a>    <a href="#fcn_eml_xscal" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xscal');" target="_self"><font color="#1122aa">eml_xscal</font></a>(<a href="#fcn_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_div');" target="_self"><font color="#1122aa">eml_div</font></a>(1.0, nrm), b_A);
<a name="153"><font color="#888888 "><i>  153   </i></font></a>    b_A[0]++;
<a name="154"><font color="#888888 "><i>  154   </i></font></a>    s[0] = -nrm;
<a name="155"><font color="#888888 "><i>  155   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="156"><font color="#888888 "><i>  156   </i></font></a>    s[0] = 0.0F;
<a name="157"><font color="#888888 "><i>  157   </i></font></a>  <b>}</b>
<a name="158"><font color="#888888 "><i>  158   </i></font></a>
<a name="159"><font color="#888888 "><i>  159   </i></font></a>  <font color="#112266"><b>if</b></font> (s[0] != 0.0F) <b>{</b>
<a name="160"><font color="#888888 "><i>  160   </i></font></a>    <a href="#fcn_eml_xaxpy" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xaxpy');" target="_self"><font color="#1122aa">eml_xaxpy</font></a>(-<a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(<a href="#fcn_eml_xdotc" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xdotc');" target="_self"><font color="#1122aa">eml_xdotc</font></a>(b_A, b_A), b_A[0]), b_A);
<a name="161"><font color="#888888 "><i>  161   </i></font></a>  <b>}</b>
<a name="162"><font color="#888888 "><i>  162   </i></font></a>
<a name="163"><font color="#888888 "><i>  163   </i></font></a>  m = 1;
<a name="164"><font color="#888888 "><i>  164   </i></font></a>  s[1] = b_A[3];
<a name="165"><font color="#888888 "><i>  165   </i></font></a>  e[0] = b_A[2];
<a name="166"><font color="#888888 "><i>  166   </i></font></a>  e[1] = 0.0F;
<a name="167"><font color="#888888 "><i>  167   </i></font></a>  <font color="#112266"><b>for</b></font> (kase = 0; kase &lt; 2; kase++) <b>{</b>
<a name="168"><font color="#888888 "><i>  168   </i></font></a>    U[kase] = b_A[kase];
<a name="169"><font color="#888888 "><i>  169   </i></font></a>    U[2 + kase] = 0.0F;
<a name="170"><font color="#888888 "><i>  170   </i></font></a>  <b>}</b>
<a name="171"><font color="#888888 "><i>  171   </i></font></a>
<a name="172"><font color="#888888 "><i>  172   </i></font></a>  U[3] = 1.0F;
<a name="173"><font color="#888888 "><i>  173   </i></font></a>  <font color="#112266"><b>if</b></font> (s[0] != 0.0F) <b>{</b>
<a name="174"><font color="#888888 "><i>  174   </i></font></a>    <a href="#fcn_eml_xaxpy" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xaxpy');" target="_self"><font color="#1122aa">eml_xaxpy</font></a>(-<a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(<a href="#fcn_eml_xdotc" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xdotc');" target="_self"><font color="#1122aa">eml_xdotc</font></a>(U, U), U[0]), U);
<a name="175"><font color="#888888 "><i>  175   </i></font></a>    <font color="#112266"><b>for</b></font> (kase = 0; kase &lt; 2; kase++) <b>{</b>
<a name="176"><font color="#888888 "><i>  176   </i></font></a>      U[kase] = -U[kase];
<a name="177"><font color="#888888 "><i>  177   </i></font></a>    <b>}</b>
<a name="178"><font color="#888888 "><i>  178   </i></font></a>
<a name="179"><font color="#888888 "><i>  179   </i></font></a>    U[0]++;
<a name="180"><font color="#888888 "><i>  180   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="181"><font color="#888888 "><i>  181   </i></font></a>    <font color="#112266"><b>for</b></font> (kase = 0; kase &lt; 2; kase++) <b>{</b>
<a name="182"><font color="#888888 "><i>  182   </i></font></a>      U[kase] = 0.0F;
<a name="183"><font color="#888888 "><i>  183   </i></font></a>    <b>}</b>
<a name="184"><font color="#888888 "><i>  184   </i></font></a>
<a name="185"><font color="#888888 "><i>  185   </i></font></a>    U[0] = 1.0F;
<a name="186"><font color="#888888 "><i>  186   </i></font></a>  <b>}</b>
<a name="187"><font color="#888888 "><i>  187   </i></font></a>
<a name="188"><font color="#888888 "><i>  188   </i></font></a>  <font color="#112266"><b>for</b></font> (q = 1; q &gt; -1; q += -1) <b>{</b>
<a name="189"><font color="#888888 "><i>  189   </i></font></a>    <font color="#112266"><b>for</b></font> (kase = 0; kase &lt; 2; kase++) <b>{</b>
<a name="190"><font color="#888888 "><i>  190   </i></font></a>      Vf[kase + (q &lt;&lt; 1)] = 0.0F;
<a name="191"><font color="#888888 "><i>  191   </i></font></a>    <b>}</b>
<a name="192"><font color="#888888 "><i>  192   </i></font></a>
<a name="193"><font color="#888888 "><i>  193   </i></font></a>    Vf[q + (q &lt;&lt; 1)] = 1.0F;
<a name="194"><font color="#888888 "><i>  194   </i></font></a>  <b>}</b>
<a name="195"><font color="#888888 "><i>  195   </i></font></a>
<a name="196"><font color="#888888 "><i>  196   </i></font></a>  nrm = b_A[2];
<a name="197"><font color="#888888 "><i>  197   </i></font></a>  <font color="#112266"><b>for</b></font> (q = 0; q &lt; 2; q++) <b>{</b>
<a name="198"><font color="#888888 "><i>  198   </i></font></a>    <font color="#112266"><b>if</b></font> (s[q] != 0.0F) <b>{</b>
<a name="199"><font color="#888888 "><i>  199   </i></font></a>      rt = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[q]);
<a name="200"><font color="#888888 "><i>  200   </i></font></a>      ztest = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(s[q], rt);
<a name="201"><font color="#888888 "><i>  201   </i></font></a>      s[q] = rt;
<a name="202"><font color="#888888 "><i>  202   </i></font></a>      <font color="#112266"><b>if</b></font> (q + 1 &lt; 2) <b>{</b>
<a name="203"><font color="#888888 "><i>  203   </i></font></a>        nrm = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(nrm, ztest);
<a name="204"><font color="#888888 "><i>  204   </i></font></a>      <b>}</b>
<a name="205"><font color="#888888 "><i>  205   </i></font></a>
<a name="206"><font color="#888888 "><i>  206   </i></font></a>      <a href="#fcn_b_eml_xscal" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_xscal');" target="_self"><font color="#1122aa">b_eml_xscal</font></a>(ztest, U, (q &lt;&lt; 1) + 1);
<a name="207"><font color="#888888 "><i>  207   </i></font></a>    <b>}</b>
<a name="208"><font color="#888888 "><i>  208   </i></font></a>
<a name="209"><font color="#888888 "><i>  209   </i></font></a>    <font color="#112266"><b>if</b></font> ((q + 1 &lt; 2) &amp;&amp; (nrm != 0.0F)) <b>{</b>
<a name="210"><font color="#888888 "><i>  210   </i></font></a>      rt = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(nrm);
<a name="211"><font color="#888888 "><i>  211   </i></font></a>      ztest = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(rt, nrm);
<a name="212"><font color="#888888 "><i>  212   </i></font></a>      nrm = rt;
<a name="213"><font color="#888888 "><i>  213   </i></font></a>      s[1] *= ztest;
<a name="214"><font color="#888888 "><i>  214   </i></font></a>      <a href="#fcn_b_eml_xscal" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_xscal');" target="_self"><font color="#1122aa">b_eml_xscal</font></a>(ztest, Vf, 3);
<a name="215"><font color="#888888 "><i>  215   </i></font></a>    <b>}</b>
<a name="216"><font color="#888888 "><i>  216   </i></font></a>
<a name="217"><font color="#888888 "><i>  217   </i></font></a>    e[0] = nrm;
<a name="218"><font color="#888888 "><i>  218   </i></font></a>  <b>}</b>
<a name="219"><font color="#888888 "><i>  219   </i></font></a>
<a name="220"><font color="#888888 "><i>  220   </i></font></a>  iter = 0;
<a name="221"><font color="#888888 "><i>  221   </i></font></a>  tiny = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(1.17549435E-38F, 1.1920929E-7F);
<a name="222"><font color="#888888 "><i>  222   </i></font></a>  snorm = 0.0F;
<a name="223"><font color="#888888 "><i>  223   </i></font></a>  <font color="#112266"><b>for</b></font> (kase = 0; kase &lt; 2; kase++) <b>{</b>
<a name="224"><font color="#888888 "><i>  224   </i></font></a>    nrm = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[kase]);
<a name="225"><font color="#888888 "><i>  225   </i></font></a>    ztest = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(e[kase]);
<a name="226"><font color="#888888 "><i>  226   </i></font></a>    <font color="#112266"><b>if</b></font> ((nrm &gt;= ztest) || <a href="rt_nonfinite_c.html#fcn_rtIsNaNF" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsNaNF');" target="_self"><font color="#1122aa">rtIsNaNF</font></a>(ztest)) <b>{</b>
<a name="227"><font color="#888888 "><i>  227   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="228"><font color="#888888 "><i>  228   </i></font></a>      nrm = ztest;
<a name="229"><font color="#888888 "><i>  229   </i></font></a>    <b>}</b>
<a name="230"><font color="#888888 "><i>  230   </i></font></a>
<a name="231"><font color="#888888 "><i>  231   </i></font></a>    <font color="#112266"><b>if</b></font> ((snorm &gt;= nrm) || <a href="rt_nonfinite_c.html#fcn_rtIsNaNF" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsNaNF');" target="_self"><font color="#1122aa">rtIsNaNF</font></a>(nrm)) <b>{</b>
<a name="232"><font color="#888888 "><i>  232   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="233"><font color="#888888 "><i>  233   </i></font></a>      snorm = nrm;
<a name="234"><font color="#888888 "><i>  234   </i></font></a>    <b>}</b>
<a name="235"><font color="#888888 "><i>  235   </i></font></a>  <b>}</b>
<a name="236"><font color="#888888 "><i>  236   </i></font></a>
<a name="237"><font color="#888888 "><i>  237   </i></font></a>  <font color="#112266"><b>while</b></font> ((m + 1 &gt; 0) &amp;&amp; (!(iter &gt;= 75))) <b>{</b>
<a name="238"><font color="#888888 "><i>  238   </i></font></a>    q = m;
<a name="239"><font color="#888888 "><i>  239   </i></font></a>    exitg3 = false;
<a name="240"><font color="#888888 "><i>  240   </i></font></a>    <font color="#112266"><b>while</b></font> (!(exitg3 || (q == 0))) <b>{</b>
<a name="241"><font color="#888888 "><i>  241   </i></font></a>      nrm = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(e[0]);
<a name="242"><font color="#888888 "><i>  242   </i></font></a>      <font color="#112266"><b>if</b></font> ((nrm &lt;= 1.1920929E-7F * ((<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[0]) + (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[1])))
<a name="243"><font color="#888888 "><i>  243   </i></font></a>          || (nrm &lt;= tiny) || ((iter &gt; 20) &amp;&amp; (nrm &lt;= 1.1920929E-7F * snorm))) <b>{</b>
<a name="244"><font color="#888888 "><i>  244   </i></font></a>        e[0] = 0.0F;
<a name="245"><font color="#888888 "><i>  245   </i></font></a>        exitg3 = true;
<a name="246"><font color="#888888 "><i>  246   </i></font></a>      <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="247"><font color="#888888 "><i>  247   </i></font></a>        q = 0;
<a name="248"><font color="#888888 "><i>  248   </i></font></a>      <b>}</b>
<a name="249"><font color="#888888 "><i>  249   </i></font></a>    <b>}</b>
<a name="250"><font color="#888888 "><i>  250   </i></font></a>
<a name="251"><font color="#888888 "><i>  251   </i></font></a>    <font color="#112266"><b>if</b></font> (q == m) <b>{</b>
<a name="252"><font color="#888888 "><i>  252   </i></font></a>      kase = 4;
<a name="253"><font color="#888888 "><i>  253   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="254"><font color="#888888 "><i>  254   </i></font></a>      qs = m + 1;
<a name="255"><font color="#888888 "><i>  255   </i></font></a>      kase = m + 1;
<a name="256"><font color="#888888 "><i>  256   </i></font></a>      exitg2 = false;
<a name="257"><font color="#888888 "><i>  257   </i></font></a>      <font color="#112266"><b>while</b></font> ((!exitg2) &amp;&amp; (kase &gt;= q)) <b>{</b>
<a name="258"><font color="#888888 "><i>  258   </i></font></a>        qs = kase;
<a name="259"><font color="#888888 "><i>  259   </i></font></a>        <font color="#112266"><b>if</b></font> (kase == q) <b>{</b>
<a name="260"><font color="#888888 "><i>  260   </i></font></a>          exitg2 = true;
<a name="261"><font color="#888888 "><i>  261   </i></font></a>        <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="262"><font color="#888888 "><i>  262   </i></font></a>          nrm = 0.0F;
<a name="263"><font color="#888888 "><i>  263   </i></font></a>          <font color="#112266"><b>if</b></font> (kase &lt; m + 1) <b>{</b>
<a name="264"><font color="#888888 "><i>  264   </i></font></a>            nrm = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(e[0]);
<a name="265"><font color="#888888 "><i>  265   </i></font></a>          <b>}</b>
<a name="266"><font color="#888888 "><i>  266   </i></font></a>
<a name="267"><font color="#888888 "><i>  267   </i></font></a>          <font color="#112266"><b>if</b></font> (kase &gt; q + 1) <b>{</b>
<a name="268"><font color="#888888 "><i>  268   </i></font></a>            nrm += (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(e[0]);
<a name="269"><font color="#888888 "><i>  269   </i></font></a>          <b>}</b>
<a name="270"><font color="#888888 "><i>  270   </i></font></a>
<a name="271"><font color="#888888 "><i>  271   </i></font></a>          ztest = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[kase - 1]);
<a name="272"><font color="#888888 "><i>  272   </i></font></a>          <font color="#112266"><b>if</b></font> ((ztest &lt;= 1.1920929E-7F * nrm) || (ztest &lt;= tiny)) <b>{</b>
<a name="273"><font color="#888888 "><i>  273   </i></font></a>            s[kase - 1] = 0.0F;
<a name="274"><font color="#888888 "><i>  274   </i></font></a>            exitg2 = true;
<a name="275"><font color="#888888 "><i>  275   </i></font></a>          <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="276"><font color="#888888 "><i>  276   </i></font></a>            kase--;
<a name="277"><font color="#888888 "><i>  277   </i></font></a>          <b>}</b>
<a name="278"><font color="#888888 "><i>  278   </i></font></a>        <b>}</b>
<a name="279"><font color="#888888 "><i>  279   </i></font></a>      <b>}</b>
<a name="280"><font color="#888888 "><i>  280   </i></font></a>
<a name="281"><font color="#888888 "><i>  281   </i></font></a>      <font color="#112266"><b>if</b></font> (qs == q) <b>{</b>
<a name="282"><font color="#888888 "><i>  282   </i></font></a>        kase = 3;
<a name="283"><font color="#888888 "><i>  283   </i></font></a>      <b>}</b> <font color="#112266"><b>else</b></font> <font color="#112266"><b>if</b></font> (qs == m + 1) <b>{</b>
<a name="284"><font color="#888888 "><i>  284   </i></font></a>        kase = 1;
<a name="285"><font color="#888888 "><i>  285   </i></font></a>      <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="286"><font color="#888888 "><i>  286   </i></font></a>        kase = 2;
<a name="287"><font color="#888888 "><i>  287   </i></font></a>        q = qs;
<a name="288"><font color="#888888 "><i>  288   </i></font></a>      <b>}</b>
<a name="289"><font color="#888888 "><i>  289   </i></font></a>    <b>}</b>
<a name="290"><font color="#888888 "><i>  290   </i></font></a>
<a name="291"><font color="#888888 "><i>  291   </i></font></a>    <font color="#112266"><b>switch</b></font> (kase) <b>{</b>
<a name="292"><font color="#888888 "><i>  292   </i></font></a>     <font color="#112266"><b>case</b></font> 1:
<a name="293"><font color="#888888 "><i>  293   </i></font></a>      f = e[0];
<a name="294"><font color="#888888 "><i>  294   </i></font></a>      e[0] = 0.0F;
<a name="295"><font color="#888888 "><i>  295   </i></font></a>      qs = m;
<a name="296"><font color="#888888 "><i>  296   </i></font></a>      <font color="#112266"><b>while</b></font> (qs &gt;= q + 1) <b>{</b>
<a name="297"><font color="#888888 "><i>  297   </i></font></a>        nrm = s[0];
<a name="298"><font color="#888888 "><i>  298   </i></font></a>        <a href="#fcn_eml_xrotg" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrotg');" target="_self"><font color="#1122aa">eml_xrotg</font></a>(&amp;nrm, &amp;f, &amp;ztest, &amp;rt);
<a name="299"><font color="#888888 "><i>  299   </i></font></a>        s[0] = nrm;
<a name="300"><font color="#888888 "><i>  300   </i></font></a>        <a href="#fcn_eml_xrot" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrot');" target="_self"><font color="#1122aa">eml_xrot</font></a>(Vf, 1, (m &lt;&lt; 1) + 1, ztest, rt);
<a name="301"><font color="#888888 "><i>  301   </i></font></a>        qs = 0;
<a name="302"><font color="#888888 "><i>  302   </i></font></a>      <b>}</b>
<a name="303"><font color="#888888 "><i>  303   </i></font></a>      <font color="#112266"><b>break</b></font>;
<a name="304"><font color="#888888 "><i>  304   </i></font></a>
<a name="305"><font color="#888888 "><i>  305   </i></font></a>     <font color="#112266"><b>case</b></font> 2:
<a name="306"><font color="#888888 "><i>  306   </i></font></a>      f = e[q - 1];
<a name="307"><font color="#888888 "><i>  307   </i></font></a>      e[q - 1] = 0.0F;
<a name="308"><font color="#888888 "><i>  308   </i></font></a>      <font color="#112266"><b>for</b></font> (qs = q; qs + 1 &lt;= m + 1; qs++) <b>{</b>
<a name="309"><font color="#888888 "><i>  309   </i></font></a>        <a href="#fcn_eml_xrotg" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrotg');" target="_self"><font color="#1122aa">eml_xrotg</font></a>(&amp;s[qs], &amp;f, &amp;ztest, &amp;rt);
<a name="310"><font color="#888888 "><i>  310   </i></font></a>        f = -rt * e[qs];
<a name="311"><font color="#888888 "><i>  311   </i></font></a>        e[qs] *= ztest;
<a name="312"><font color="#888888 "><i>  312   </i></font></a>        <a href="#fcn_eml_xrot" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrot');" target="_self"><font color="#1122aa">eml_xrot</font></a>(U, (qs &lt;&lt; 1) + 1, ((q - 1) &lt;&lt; 1) + 1, ztest, rt);
<a name="313"><font color="#888888 "><i>  313   </i></font></a>      <b>}</b>
<a name="314"><font color="#888888 "><i>  314   </i></font></a>      <font color="#112266"><b>break</b></font>;
<a name="315"><font color="#888888 "><i>  315   </i></font></a>
<a name="316"><font color="#888888 "><i>  316   </i></font></a>     <font color="#112266"><b>case</b></font> 3:
<a name="317"><font color="#888888 "><i>  317   </i></font></a>      varargin_1[0] = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[m]);
<a name="318"><font color="#888888 "><i>  318   </i></font></a>      varargin_1[1] = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[m - 1]);
<a name="319"><font color="#888888 "><i>  319   </i></font></a>      varargin_1[2] = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(e[m - 1]);
<a name="320"><font color="#888888 "><i>  320   </i></font></a>      varargin_1[3] = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(s[q]);
<a name="321"><font color="#888888 "><i>  321   </i></font></a>      varargin_1[4] = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(e[q]);
<a name="322"><font color="#888888 "><i>  322   </i></font></a>      kase = 1;
<a name="323"><font color="#888888 "><i>  323   </i></font></a>      mtmp = varargin_1[0];
<a name="324"><font color="#888888 "><i>  324   </i></font></a>      <font color="#112266"><b>if</b></font> (<a href="rt_nonfinite_c.html#fcn_rtIsNaNF" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsNaNF');" target="_self"><font color="#1122aa">rtIsNaNF</font></a>(varargin_1[0])) <b>{</b>
<a name="325"><font color="#888888 "><i>  325   </i></font></a>        qs = 2;
<a name="326"><font color="#888888 "><i>  326   </i></font></a>        exitg1 = false;
<a name="327"><font color="#888888 "><i>  327   </i></font></a>        <font color="#112266"><b>while</b></font> ((!exitg1) &amp;&amp; (qs &lt; 6)) <b>{</b>
<a name="328"><font color="#888888 "><i>  328   </i></font></a>          kase = qs;
<a name="329"><font color="#888888 "><i>  329   </i></font></a>          <font color="#112266"><b>if</b></font> (!<a href="rt_nonfinite_c.html#fcn_rtIsNaNF" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsNaNF');" target="_self"><font color="#1122aa">rtIsNaNF</font></a>(varargin_1[qs - 1])) <b>{</b>
<a name="330"><font color="#888888 "><i>  330   </i></font></a>            mtmp = varargin_1[qs - 1];
<a name="331"><font color="#888888 "><i>  331   </i></font></a>            exitg1 = true;
<a name="332"><font color="#888888 "><i>  332   </i></font></a>          <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="333"><font color="#888888 "><i>  333   </i></font></a>            qs++;
<a name="334"><font color="#888888 "><i>  334   </i></font></a>          <b>}</b>
<a name="335"><font color="#888888 "><i>  335   </i></font></a>        <b>}</b>
<a name="336"><font color="#888888 "><i>  336   </i></font></a>      <b>}</b>
<a name="337"><font color="#888888 "><i>  337   </i></font></a>
<a name="338"><font color="#888888 "><i>  338   </i></font></a>      <font color="#112266"><b>if</b></font> (kase &lt; 5) <b>{</b>
<a name="339"><font color="#888888 "><i>  339   </i></font></a>        <font color="#112266"><b>while</b></font> (kase + 1 &lt; 6) <b>{</b>
<a name="340"><font color="#888888 "><i>  340   </i></font></a>          <font color="#112266"><b>if</b></font> (varargin_1[kase] &gt; mtmp) <b>{</b>
<a name="341"><font color="#888888 "><i>  341   </i></font></a>            mtmp = varargin_1[kase];
<a name="342"><font color="#888888 "><i>  342   </i></font></a>          <b>}</b>
<a name="343"><font color="#888888 "><i>  343   </i></font></a>
<a name="344"><font color="#888888 "><i>  344   </i></font></a>          kase++;
<a name="345"><font color="#888888 "><i>  345   </i></font></a>        <b>}</b>
<a name="346"><font color="#888888 "><i>  346   </i></font></a>      <b>}</b>
<a name="347"><font color="#888888 "><i>  347   </i></font></a>
<a name="348"><font color="#888888 "><i>  348   </i></font></a>      f = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(s[m], mtmp);
<a name="349"><font color="#888888 "><i>  349   </i></font></a>      nrm = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(s[0], mtmp);
<a name="350"><font color="#888888 "><i>  350   </i></font></a>      ztest = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(e[0], mtmp);
<a name="351"><font color="#888888 "><i>  351   </i></font></a>      sqds = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(s[q], mtmp);
<a name="352"><font color="#888888 "><i>  352   </i></font></a>      rt = <a href="#fcn_c_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_c_eml_div');" target="_self"><font color="#1122aa">c_eml_div</font></a>((nrm + f) * (nrm - f) + ztest * ztest, 2.0);
<a name="353"><font color="#888888 "><i>  353   </i></font></a>      nrm = f * ztest;
<a name="354"><font color="#888888 "><i>  354   </i></font></a>      nrm *= nrm;
<a name="355"><font color="#888888 "><i>  355   </i></font></a>      ztest = 0.0F;
<a name="356"><font color="#888888 "><i>  356   </i></font></a>      <font color="#112266"><b>if</b></font> ((rt != 0.0F) || (nrm != 0.0F)) <b>{</b>
<a name="357"><font color="#888888 "><i>  357   </i></font></a>        ztest = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)sqrt(rt * rt + nrm);
<a name="358"><font color="#888888 "><i>  358   </i></font></a>        <font color="#112266"><b>if</b></font> (rt &lt; 0.0F) <b>{</b>
<a name="359"><font color="#888888 "><i>  359   </i></font></a>          ztest = -ztest;
<a name="360"><font color="#888888 "><i>  360   </i></font></a>        <b>}</b>
<a name="361"><font color="#888888 "><i>  361   </i></font></a>
<a name="362"><font color="#888888 "><i>  362   </i></font></a>        ztest = <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(nrm, rt + ztest);
<a name="363"><font color="#888888 "><i>  363   </i></font></a>      <b>}</b>
<a name="364"><font color="#888888 "><i>  364   </i></font></a>
<a name="365"><font color="#888888 "><i>  365   </i></font></a>      f = (sqds + f) * (sqds - f) + ztest;
<a name="366"><font color="#888888 "><i>  366   </i></font></a>      nrm = sqds * <a href="svd_c.html#fcn_b_eml_div" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_div');" target="_self"><font color="#1122aa">b_eml_div</font></a>(e[q], mtmp);
<a name="367"><font color="#888888 "><i>  367   </i></font></a>      <font color="#112266"><b>while</b></font> (q + 1 &lt;= m) <b>{</b>
<a name="368"><font color="#888888 "><i>  368   </i></font></a>        <a href="#fcn_eml_xrotg" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrotg');" target="_self"><font color="#1122aa">eml_xrotg</font></a>(&amp;f, &amp;nrm, &amp;ztest, &amp;rt);
<a name="369"><font color="#888888 "><i>  369   </i></font></a>        f = ztest * s[0] + rt * e[0];
<a name="370"><font color="#888888 "><i>  370   </i></font></a>        e[0] = ztest * e[0] - rt * s[0];
<a name="371"><font color="#888888 "><i>  371   </i></font></a>        nrm = rt * s[1];
<a name="372"><font color="#888888 "><i>  372   </i></font></a>        s[1] *= ztest;
<a name="373"><font color="#888888 "><i>  373   </i></font></a>        <a href="#fcn_eml_xrot" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrot');" target="_self"><font color="#1122aa">eml_xrot</font></a>(Vf, 1, 3, ztest, rt);
<a name="374"><font color="#888888 "><i>  374   </i></font></a>        s[0] = f;
<a name="375"><font color="#888888 "><i>  375   </i></font></a>        <a href="#fcn_eml_xrotg" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrotg');" target="_self"><font color="#1122aa">eml_xrotg</font></a>(&amp;s[0], &amp;nrm, &amp;ztest, &amp;rt);
<a name="376"><font color="#888888 "><i>  376   </i></font></a>        f = ztest * e[0] + rt * s[1];
<a name="377"><font color="#888888 "><i>  377   </i></font></a>        s[1] = -rt * e[0] + ztest * s[1];
<a name="378"><font color="#888888 "><i>  378   </i></font></a>        nrm = rt * e[1];
<a name="379"><font color="#888888 "><i>  379   </i></font></a>        e[1] *= ztest;
<a name="380"><font color="#888888 "><i>  380   </i></font></a>        <a href="#fcn_eml_xrot" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xrot');" target="_self"><font color="#1122aa">eml_xrot</font></a>(U, 1, 3, ztest, rt);
<a name="381"><font color="#888888 "><i>  381   </i></font></a>        q = 1;
<a name="382"><font color="#888888 "><i>  382   </i></font></a>      <b>}</b>
<a name="383"><font color="#888888 "><i>  383   </i></font></a>
<a name="384"><font color="#888888 "><i>  384   </i></font></a>      e[0] = f;
<a name="385"><font color="#888888 "><i>  385   </i></font></a>      iter++;
<a name="386"><font color="#888888 "><i>  386   </i></font></a>      <font color="#112266"><b>break</b></font>;
<a name="387"><font color="#888888 "><i>  387   </i></font></a>
<a name="388"><font color="#888888 "><i>  388   </i></font></a>     <font color="#112266"><b>default</b></font>:
<a name="389"><font color="#888888 "><i>  389   </i></font></a>      <font color="#112266"><b>if</b></font> (s[q] &lt; 0.0F) <b>{</b>
<a name="390"><font color="#888888 "><i>  390   </i></font></a>        s[q] = -s[q];
<a name="391"><font color="#888888 "><i>  391   </i></font></a>        <a href="#fcn_b_eml_xscal" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_eml_xscal');" target="_self"><font color="#1122aa">b_eml_xscal</font></a>(-1.0F, Vf, (q &lt;&lt; 1) + 1);
<a name="392"><font color="#888888 "><i>  392   </i></font></a>      <b>}</b>
<a name="393"><font color="#888888 "><i>  393   </i></font></a>
<a name="394"><font color="#888888 "><i>  394   </i></font></a>      <font color="#112266"><b>while</b></font> ((q + 1 &lt; 2) &amp;&amp; (s[0] &lt; s[1])) <b>{</b>
<a name="395"><font color="#888888 "><i>  395   </i></font></a>        rt = s[0];
<a name="396"><font color="#888888 "><i>  396   </i></font></a>        s[0] = s[1];
<a name="397"><font color="#888888 "><i>  397   </i></font></a>        s[1] = rt;
<a name="398"><font color="#888888 "><i>  398   </i></font></a>        <a href="#fcn_eml_xswap" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xswap');" target="_self"><font color="#1122aa">eml_xswap</font></a>(Vf, 1, 3);
<a name="399"><font color="#888888 "><i>  399   </i></font></a>        <a href="#fcn_eml_xswap" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xswap');" target="_self"><font color="#1122aa">eml_xswap</font></a>(U, 1, 3);
<a name="400"><font color="#888888 "><i>  400   </i></font></a>        q = 1;
<a name="401"><font color="#888888 "><i>  401   </i></font></a>      <b>}</b>
<a name="402"><font color="#888888 "><i>  402   </i></font></a>
<a name="403"><font color="#888888 "><i>  403   </i></font></a>      iter = 0;
<a name="404"><font color="#888888 "><i>  404   </i></font></a>      m--;
<a name="405"><font color="#888888 "><i>  405   </i></font></a>      <font color="#112266"><b>break</b></font>;
<a name="406"><font color="#888888 "><i>  406   </i></font></a>    <b>}</b>
<a name="407"><font color="#888888 "><i>  407   </i></font></a>  <b>}</b>
<a name="408"><font color="#888888 "><i>  408   </i></font></a>
<a name="409"><font color="#888888 "><i>  409   </i></font></a>  <font color="#112266"><b>for</b></font> (qs = 0; qs &lt; 2; qs++) <b>{</b>
<a name="410"><font color="#888888 "><i>  410   </i></font></a>    S[qs] = s[qs];
<a name="411"><font color="#888888 "><i>  411   </i></font></a>    <font color="#112266"><b>for</b></font> (kase = 0; kase &lt; 2; kase++) <b>{</b>
<a name="412"><font color="#888888 "><i>  412   </i></font></a>      V[kase + (qs &lt;&lt; 1)] = Vf[kase + (qs &lt;&lt; 1)];
<a name="413"><font color="#888888 "><i>  413   </i></font></a>    <b>}</b>
<a name="414"><font color="#888888 "><i>  414   </i></font></a>  <b>}</b>
<a name="415"><font color="#888888 "><i>  415   </i></font></a><b>}</b>
<a name="416"><font color="#888888 "><i>  416   </i></font></a>
<a name="417"><font color="#888888 "><i>  417   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="418"><font color="#888888 "><i>  418   </i></font></a><font color="#117755"><I> * Arguments    : const float x[4]</i></font>
<a name="419"><font color="#888888 "><i>  419   </i></font></a><font color="#117755"><I> * Return Type  : float</i></font>
<a name="420"><font color="#888888 "><i>  420   </i></font></a><font color="#117755"><I> */</i></font>
<a name="421"><font color="#888888 "><i>  421   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="fcn_eml_xnrm2">eml_xnrm2</a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[4])
<a name="422"><font color="#888888 "><i>  422   </i></font></a><b>{</b>
<a name="423"><font color="#888888 "><i>  423   </i></font></a>  <font color="#112266"><b>float</b></font> y;
<a name="424"><font color="#888888 "><i>  424   </i></font></a>  <font color="#112266"><b>float</b></font> scale;
<a name="425"><font color="#888888 "><i>  425   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="426"><font color="#888888 "><i>  426   </i></font></a>  <font color="#112266"><b>float</b></font> absxk;
<a name="427"><font color="#888888 "><i>  427   </i></font></a>  <font color="#112266"><b>float</b></font> t;
<a name="428"><font color="#888888 "><i>  428   </i></font></a>  y = 0.0F;
<a name="429"><font color="#888888 "><i>  429   </i></font></a>  scale = 1.17549435E-38F;
<a name="430"><font color="#888888 "><i>  430   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 2; k++) <b>{</b>
<a name="431"><font color="#888888 "><i>  431   </i></font></a>    absxk = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(x[k]);
<a name="432"><font color="#888888 "><i>  432   </i></font></a>    <font color="#112266"><b>if</b></font> (absxk &gt; scale) <b>{</b>
<a name="433"><font color="#888888 "><i>  433   </i></font></a>      t = scale / absxk;
<a name="434"><font color="#888888 "><i>  434   </i></font></a>      y = 1.0F + y * t * t;
<a name="435"><font color="#888888 "><i>  435   </i></font></a>      scale = absxk;
<a name="436"><font color="#888888 "><i>  436   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="437"><font color="#888888 "><i>  437   </i></font></a>      t = absxk / scale;
<a name="438"><font color="#888888 "><i>  438   </i></font></a>      y += t * t;
<a name="439"><font color="#888888 "><i>  439   </i></font></a>    <b>}</b>
<a name="440"><font color="#888888 "><i>  440   </i></font></a>  <b>}</b>
<a name="441"><font color="#888888 "><i>  441   </i></font></a>
<a name="442"><font color="#888888 "><i>  442   </i></font></a>  <font color="#112266"><b>return</b></font> scale * (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)sqrt(y);
<a name="443"><font color="#888888 "><i>  443   </i></font></a><b>}</b>
<a name="444"><font color="#888888 "><i>  444   </i></font></a>
<a name="445"><font color="#888888 "><i>  445   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="446"><font color="#888888 "><i>  446   </i></font></a><font color="#117755"><I> * Arguments    : float x[4]</i></font>
<a name="447"><font color="#888888 "><i>  447   </i></font></a><font color="#117755"><I> *                int ix0</i></font>
<a name="448"><font color="#888888 "><i>  448   </i></font></a><font color="#117755"><I> *                int iy0</i></font>
<a name="449"><font color="#888888 "><i>  449   </i></font></a><font color="#117755"><I> *                float c</i></font>
<a name="450"><font color="#888888 "><i>  450   </i></font></a><font color="#117755"><I> *                float s</i></font>
<a name="451"><font color="#888888 "><i>  451   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="452"><font color="#888888 "><i>  452   </i></font></a><font color="#117755"><I> */</i></font>
<a name="453"><font color="#888888 "><i>  453   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_eml_xrot">eml_xrot</a>(<font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>int</b></font> ix0, <font color="#112266"><b>int</b></font> iy0, <font color="#112266"><b>float</b></font> c, <font color="#112266"><b>float</b></font> s)
<a name="454"><font color="#888888 "><i>  454   </i></font></a><b>{</b>
<a name="455"><font color="#888888 "><i>  455   </i></font></a>  <font color="#112266"><b>int</b></font> ix;
<a name="456"><font color="#888888 "><i>  456   </i></font></a>  <font color="#112266"><b>int</b></font> iy;
<a name="457"><font color="#888888 "><i>  457   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="458"><font color="#888888 "><i>  458   </i></font></a>  <font color="#112266"><b>float</b></font> temp;
<a name="459"><font color="#888888 "><i>  459   </i></font></a>  ix = ix0 - 1;
<a name="460"><font color="#888888 "><i>  460   </i></font></a>  iy = iy0 - 1;
<a name="461"><font color="#888888 "><i>  461   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 2; k++) <b>{</b>
<a name="462"><font color="#888888 "><i>  462   </i></font></a>    temp = c * x[ix] + s * x[iy];
<a name="463"><font color="#888888 "><i>  463   </i></font></a>    x[iy] = c * x[iy] - s * x[ix];
<a name="464"><font color="#888888 "><i>  464   </i></font></a>    x[ix] = temp;
<a name="465"><font color="#888888 "><i>  465   </i></font></a>    iy++;
<a name="466"><font color="#888888 "><i>  466   </i></font></a>    ix++;
<a name="467"><font color="#888888 "><i>  467   </i></font></a>  <b>}</b>
<a name="468"><font color="#888888 "><i>  468   </i></font></a><b>}</b>
<a name="469"><font color="#888888 "><i>  469   </i></font></a>
<a name="470"><font color="#888888 "><i>  470   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="471"><font color="#888888 "><i>  471   </i></font></a><font color="#117755"><I> * Arguments    : float *a</i></font>
<a name="472"><font color="#888888 "><i>  472   </i></font></a><font color="#117755"><I> *                float *b</i></font>
<a name="473"><font color="#888888 "><i>  473   </i></font></a><font color="#117755"><I> *                float *c</i></font>
<a name="474"><font color="#888888 "><i>  474   </i></font></a><font color="#117755"><I> *                float *s</i></font>
<a name="475"><font color="#888888 "><i>  475   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="476"><font color="#888888 "><i>  476   </i></font></a><font color="#117755"><I> */</i></font>
<a name="477"><font color="#888888 "><i>  477   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_eml_xrotg">eml_xrotg</a>(<font color="#112266"><b>float</b></font> *a, <font color="#112266"><b>float</b></font> *b, <font color="#112266"><b>float</b></font> *c, <font color="#112266"><b>float</b></font> *s)
<a name="478"><font color="#888888 "><i>  478   </i></font></a><b>{</b>
<a name="479"><font color="#888888 "><i>  479   </i></font></a>  <font color="#112266"><b>float</b></font> roe;
<a name="480"><font color="#888888 "><i>  480   </i></font></a>  <font color="#112266"><b>float</b></font> absa;
<a name="481"><font color="#888888 "><i>  481   </i></font></a>  <font color="#112266"><b>float</b></font> absb;
<a name="482"><font color="#888888 "><i>  482   </i></font></a>  <font color="#112266"><b>float</b></font> scale;
<a name="483"><font color="#888888 "><i>  483   </i></font></a>  <font color="#112266"><b>float</b></font> ads;
<a name="484"><font color="#888888 "><i>  484   </i></font></a>  <font color="#112266"><b>float</b></font> bds;
<a name="485"><font color="#888888 "><i>  485   </i></font></a>  roe = *b;
<a name="486"><font color="#888888 "><i>  486   </i></font></a>  absa = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(*a);
<a name="487"><font color="#888888 "><i>  487   </i></font></a>  absb = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(*b);
<a name="488"><font color="#888888 "><i>  488   </i></font></a>  <font color="#112266"><b>if</b></font> (absa &gt; absb) <b>{</b>
<a name="489"><font color="#888888 "><i>  489   </i></font></a>    roe = *a;
<a name="490"><font color="#888888 "><i>  490   </i></font></a>  <b>}</b>
<a name="491"><font color="#888888 "><i>  491   </i></font></a>
<a name="492"><font color="#888888 "><i>  492   </i></font></a>  scale = absa + absb;
<a name="493"><font color="#888888 "><i>  493   </i></font></a>  <font color="#112266"><b>if</b></font> (scale == 0.0F) <b>{</b>
<a name="494"><font color="#888888 "><i>  494   </i></font></a>    *s = 0.0F;
<a name="495"><font color="#888888 "><i>  495   </i></font></a>    *c = 1.0F;
<a name="496"><font color="#888888 "><i>  496   </i></font></a>    ads = 0.0F;
<a name="497"><font color="#888888 "><i>  497   </i></font></a>    scale = 0.0F;
<a name="498"><font color="#888888 "><i>  498   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="499"><font color="#888888 "><i>  499   </i></font></a>    ads = absa / scale;
<a name="500"><font color="#888888 "><i>  500   </i></font></a>    bds = absb / scale;
<a name="501"><font color="#888888 "><i>  501   </i></font></a>    ads = scale * (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)sqrt(ads * ads + bds * bds);
<a name="502"><font color="#888888 "><i>  502   </i></font></a>    <font color="#112266"><b>if</b></font> (roe &lt; 0.0F) <b>{</b>
<a name="503"><font color="#888888 "><i>  503   </i></font></a>      ads = -ads;
<a name="504"><font color="#888888 "><i>  504   </i></font></a>    <b>}</b>
<a name="505"><font color="#888888 "><i>  505   </i></font></a>
<a name="506"><font color="#888888 "><i>  506   </i></font></a>    *c = *a / ads;
<a name="507"><font color="#888888 "><i>  507   </i></font></a>    *s = *b / ads;
<a name="508"><font color="#888888 "><i>  508   </i></font></a>    <font color="#112266"><b>if</b></font> (absa &gt; absb) <b>{</b>
<a name="509"><font color="#888888 "><i>  509   </i></font></a>      scale = *s;
<a name="510"><font color="#888888 "><i>  510   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <font color="#112266"><b>if</b></font> (*c != 0.0F) <b>{</b>
<a name="511"><font color="#888888 "><i>  511   </i></font></a>      scale = 1.0F / *c;
<a name="512"><font color="#888888 "><i>  512   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="513"><font color="#888888 "><i>  513   </i></font></a>      scale = 1.0F;
<a name="514"><font color="#888888 "><i>  514   </i></font></a>    <b>}</b>
<a name="515"><font color="#888888 "><i>  515   </i></font></a>  <b>}</b>
<a name="516"><font color="#888888 "><i>  516   </i></font></a>
<a name="517"><font color="#888888 "><i>  517   </i></font></a>  *a = ads;
<a name="518"><font color="#888888 "><i>  518   </i></font></a>  *b = scale;
<a name="519"><font color="#888888 "><i>  519   </i></font></a><b>}</b>
<a name="520"><font color="#888888 "><i>  520   </i></font></a>
<a name="521"><font color="#888888 "><i>  521   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="522"><font color="#888888 "><i>  522   </i></font></a><font color="#117755"><I> * Arguments    : float a</i></font>
<a name="523"><font color="#888888 "><i>  523   </i></font></a><font color="#117755"><I> *                float x[4]</i></font>
<a name="524"><font color="#888888 "><i>  524   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="525"><font color="#888888 "><i>  525   </i></font></a><font color="#117755"><I> */</i></font>
<a name="526"><font color="#888888 "><i>  526   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_eml_xscal">eml_xscal</a>(<font color="#112266"><b>float</b></font> a, <font color="#112266"><b>float</b></font> x[4])
<a name="527"><font color="#888888 "><i>  527   </i></font></a><b>{</b>
<a name="528"><font color="#888888 "><i>  528   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="529"><font color="#888888 "><i>  529   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 2; k++) <b>{</b>
<a name="530"><font color="#888888 "><i>  530   </i></font></a>    x[k] *= a;
<a name="531"><font color="#888888 "><i>  531   </i></font></a>  <b>}</b>
<a name="532"><font color="#888888 "><i>  532   </i></font></a><b>}</b>
<a name="533"><font color="#888888 "><i>  533   </i></font></a>
<a name="534"><font color="#888888 "><i>  534   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="535"><font color="#888888 "><i>  535   </i></font></a><font color="#117755"><I> * Arguments    : float x[4]</i></font>
<a name="536"><font color="#888888 "><i>  536   </i></font></a><font color="#117755"><I> *                int ix0</i></font>
<a name="537"><font color="#888888 "><i>  537   </i></font></a><font color="#117755"><I> *                int iy0</i></font>
<a name="538"><font color="#888888 "><i>  538   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="539"><font color="#888888 "><i>  539   </i></font></a><font color="#117755"><I> */</i></font>
<a name="540"><font color="#888888 "><i>  540   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_eml_xswap">eml_xswap</a>(<font color="#112266"><b>float</b></font> x[4], <font color="#112266"><b>int</b></font> ix0, <font color="#112266"><b>int</b></font> iy0)
<a name="541"><font color="#888888 "><i>  541   </i></font></a><b>{</b>
<a name="542"><font color="#888888 "><i>  542   </i></font></a>  <font color="#112266"><b>int</b></font> ix;
<a name="543"><font color="#888888 "><i>  543   </i></font></a>  <font color="#112266"><b>int</b></font> iy;
<a name="544"><font color="#888888 "><i>  544   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="545"><font color="#888888 "><i>  545   </i></font></a>  <font color="#112266"><b>float</b></font> temp;
<a name="546"><font color="#888888 "><i>  546   </i></font></a>  ix = ix0 - 1;
<a name="547"><font color="#888888 "><i>  547   </i></font></a>  iy = iy0 - 1;
<a name="548"><font color="#888888 "><i>  548   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 2; k++) <b>{</b>
<a name="549"><font color="#888888 "><i>  549   </i></font></a>    temp = x[ix];
<a name="550"><font color="#888888 "><i>  550   </i></font></a>    x[ix] = x[iy];
<a name="551"><font color="#888888 "><i>  551   </i></font></a>    x[iy] = temp;
<a name="552"><font color="#888888 "><i>  552   </i></font></a>    ix++;
<a name="553"><font color="#888888 "><i>  553   </i></font></a>    iy++;
<a name="554"><font color="#888888 "><i>  554   </i></font></a>  <b>}</b>
<a name="555"><font color="#888888 "><i>  555   </i></font></a><b>}</b>
<a name="556"><font color="#888888 "><i>  556   </i></font></a>
<a name="557"><font color="#888888 "><i>  557   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="558"><font color="#888888 "><i>  558   </i></font></a><font color="#117755"><I> * 作者 ytf</i></font>
<a name="559"><font color="#888888 "><i>  559   </i></font></a><font color="#117755"><I> * 日期 2017年4月27日</i></font>
<a name="560"><font color="#888888 "><i>  560   </i></font></a><font color="#117755"><I> * 状态转移协方差矩阵</i></font>
<a name="561"><font color="#888888 "><i>  561   </i></font></a><font color="#117755"><I> * H 预测矩阵</i></font>
<a name="562"><font color="#888888 "><i>  562   </i></font></a><font color="#117755"><I> * R 观测噪声方差</i></font>
<a name="563"><font color="#888888 "><i>  563   </i></font></a><font color="#117755"><I> * F 状态转移矩阵</i></font>
<a name="564"><font color="#888888 "><i>  564   </i></font></a><font color="#117755"><I> * I</i></font>
<a name="565"><font color="#888888 "><i>  565   </i></font></a><font color="#117755"><I> * Z 测量矩阵</i></font>
<a name="566"><font color="#888888 "><i>  566   </i></font></a><font color="#117755"><I> * Arguments    : const float Q[9]</i></font>
<a name="567"><font color="#888888 "><i>  567   </i></font></a><font color="#117755"><I> *                const float H[6]</i></font>
<a name="568"><font color="#888888 "><i>  568   </i></font></a><font color="#117755"><I> *                const float R[4]</i></font>
<a name="569"><font color="#888888 "><i>  569   </i></font></a><font color="#117755"><I> *                const float F[9]</i></font>
<a name="570"><font color="#888888 "><i>  570   </i></font></a><font color="#117755"><I> *                const float I[9]</i></font>
<a name="571"><font color="#888888 "><i>  571   </i></font></a><font color="#117755"><I> *                const float Z[2]</i></font>
<a name="572"><font color="#888888 "><i>  572   </i></font></a><font color="#117755"><I> *                unsigned char up</i></font>
<a name="573"><font color="#888888 "><i>  573   </i></font></a><font color="#117755"><I> *                float *h</i></font>
<a name="574"><font color="#888888 "><i>  574   </i></font></a><font color="#117755"><I> *                float *v</i></font>
<a name="575"><font color="#888888 "><i>  575   </i></font></a><font color="#117755"><I> *                float *a</i></font>
<a name="576"><font color="#888888 "><i>  576   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="577"><font color="#888888 "><i>  577   </i></font></a><font color="#117755"><I> */</i></font>
<a name="578"><font color="#888888 "><i>  578   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_nav_h_ekf">nav_h_ekf</a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> Q[9], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> H[6], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> R[4], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font>
<a name="579"><font color="#888888 "><i>  579   </i></font></a>               F[9], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> I[9], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> Z[2], <font color="#112266"><b>unsigned</b></font> <font color="#112266"><b>char</b></font> up, <font color="#112266"><b>float</b></font>
<a name="580"><font color="#888888 "><i>  580   </i></font></a>               *h, <font color="#112266"><b>float</b></font> *v, <font color="#112266"><b>float</b></font> *a)
<a name="581"><font color="#888888 "><i>  581   </i></font></a><b>{</b>
<a name="582"><font color="#888888 "><i>  582   </i></font></a>  <font color="#112266"><b>float</b></font> K[3];
<a name="583"><font color="#888888 "><i>  583   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="584"><font color="#888888 "><i>  584   </i></font></a>  <font color="#112266"><b>float</b></font> b_F[9];
<a name="585"><font color="#888888 "><i>  585   </i></font></a>  <font color="#112266"><b>int</b></font> i0;
<a name="586"><font color="#888888 "><i>  586   </i></font></a>  <font color="#112266"><b>int</b></font> vcol;
<a name="587"><font color="#888888 "><i>  587   </i></font></a>  <font color="#112266"><b>float</b></font> tol;
<a name="588"><font color="#888888 "><i>  588   </i></font></a>  <font color="#112266"><b>float</b></font> y[6];
<a name="589"><font color="#888888 "><i>  589   </i></font></a>  <font color="#112266"><b>float</b></font> b_X[4];
<a name="590"><font color="#888888 "><i>  590   </i></font></a>  <font color="#112266"><b>float</b></font> b_H[6];
<a name="591"><font color="#888888 "><i>  591   </i></font></a>  <font color="#112266"><b>float</b></font> c_H[4];
<a name="592"><font color="#888888 "><i>  592   </i></font></a>  <font color="#112266"><b>float</b></font> V[4];
<a name="593"><font color="#888888 "><i>  593   </i></font></a>  <font color="#112266"><b>float</b></font> s[2];
<a name="594"><font color="#888888 "><i>  594   </i></font></a>  <font color="#112266"><b>float</b></font> U[4];
<a name="595"><font color="#888888 "><i>  595   </i></font></a>  <font color="#112266"><b>int</b></font> r;
<a name="596"><font color="#888888 "><i>  596   </i></font></a>  <font color="#112266"><b>int</b></font> ar;
<a name="597"><font color="#888888 "><i>  597   </i></font></a>  <font color="#112266"><b>int</b></font> ic;
<a name="598"><font color="#888888 "><i>  598   </i></font></a>  <font color="#112266"><b>int</b></font> ib;
<a name="599"><font color="#888888 "><i>  599   </i></font></a>  <font color="#112266"><b>int</b></font> ia;
<a name="600"><font color="#888888 "><i>  600   </i></font></a>  <font color="#112266"><b>float</b></font> b_K[6];
<a name="601"><font color="#888888 "><i>  601   </i></font></a>  <font color="#112266"><b>float</b></font> b_I[9];
<a name="602"><font color="#888888 "><i>  602   </i></font></a>  <font color="#112266"><b>float</b></font> c_X;
<a name="603"><font color="#888888 "><i>  603   </i></font></a>  <font color="#112266"><b>float</b></font> d_H[3];
<a name="604"><font color="#888888 "><i>  604   </i></font></a>  <font color="#112266"><b>float</b></font> b_V;
<a name="605"><font color="#888888 "><i>  605   </i></font></a>  <font color="#112266"><b>float</b></font> S;
<a name="606"><font color="#888888 "><i>  606   </i></font></a>  <font color="#112266"><b>float</b></font> b_U;
<a name="607"><font color="#888888 "><i>  607   </i></font></a>
<a name="608"><font color="#888888 "><i>  608   </i></font></a>  <font color="#117755"><i>/*  状态变量 */</i></font>
<a name="609"><font color="#888888 "><i>  609   </i></font></a>  <font color="#117755"><i>/*  状态协方差矩阵 */</i></font>
<a name="610"><font color="#888888 "><i>  610   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="611"><font color="#888888 "><i>  611   </i></font></a>    K[i] = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i];
<a name="612"><font color="#888888 "><i>  612   </i></font></a>  <b>}</b>
<a name="613"><font color="#888888 "><i>  613   </i></font></a>
<a name="614"><font color="#888888 "><i>  614   </i></font></a>  <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="615"><font color="#888888 "><i>  615   </i></font></a>    <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i0] = 0.0F;
<a name="616"><font color="#888888 "><i>  616   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="617"><font color="#888888 "><i>  617   </i></font></a>      <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i0] += F[i0 + 3 * i] * K[i];
<a name="618"><font color="#888888 "><i>  618   </i></font></a>    <b>}</b>
<a name="619"><font color="#888888 "><i>  619   </i></font></a>
<a name="620"><font color="#888888 "><i>  620   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="621"><font color="#888888 "><i>  621   </i></font></a>      b_F[i0 + 3 * i] = 0.0F;
<a name="622"><font color="#888888 "><i>  622   </i></font></a>      <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 3; vcol++) <b>{</b>
<a name="623"><font color="#888888 "><i>  623   </i></font></a>        b_F[i0 + 3 * i] += F[i0 + 3 * vcol] * <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[vcol + 3 * i];
<a name="624"><font color="#888888 "><i>  624   </i></font></a>      <b>}</b>
<a name="625"><font color="#888888 "><i>  625   </i></font></a>    <b>}</b>
<a name="626"><font color="#888888 "><i>  626   </i></font></a>  <b>}</b>
<a name="627"><font color="#888888 "><i>  627   </i></font></a>
<a name="628"><font color="#888888 "><i>  628   </i></font></a>  <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="629"><font color="#888888 "><i>  629   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="630"><font color="#888888 "><i>  630   </i></font></a>      tol = 0.0F;
<a name="631"><font color="#888888 "><i>  631   </i></font></a>      <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 3; vcol++) <b>{</b>
<a name="632"><font color="#888888 "><i>  632   </i></font></a>        tol += b_F[i0 + 3 * vcol] * F[i + 3 * vcol];
<a name="633"><font color="#888888 "><i>  633   </i></font></a>      <b>}</b>
<a name="634"><font color="#888888 "><i>  634   </i></font></a>
<a name="635"><font color="#888888 "><i>  635   </i></font></a>      <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0 + 3 * i] = tol + Q[i0 + 3 * i];
<a name="636"><font color="#888888 "><i>  636   </i></font></a>    <b>}</b>
<a name="637"><font color="#888888 "><i>  637   </i></font></a>  <b>}</b>
<a name="638"><font color="#888888 "><i>  638   </i></font></a>
<a name="639"><font color="#888888 "><i>  639   </i></font></a>  <font color="#112266"><b>if</b></font> (up == 1) <b>{</b>
<a name="640"><font color="#888888 "><i>  640   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="641"><font color="#888888 "><i>  641   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="642"><font color="#888888 "><i>  642   </i></font></a>        y[i0 + 3 * i] = 0.0F;
<a name="643"><font color="#888888 "><i>  643   </i></font></a>        <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 3; vcol++) <b>{</b>
<a name="644"><font color="#888888 "><i>  644   </i></font></a>          y[i0 + 3 * i] += <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0 + 3 * vcol] * H[i + (vcol &lt;&lt; 1)];
<a name="645"><font color="#888888 "><i>  645   </i></font></a>        <b>}</b>
<a name="646"><font color="#888888 "><i>  646   </i></font></a>      <b>}</b>
<a name="647"><font color="#888888 "><i>  647   </i></font></a>    <b>}</b>
<a name="648"><font color="#888888 "><i>  648   </i></font></a>
<a name="649"><font color="#888888 "><i>  649   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 4; i0++) <b>{</b>
<a name="650"><font color="#888888 "><i>  650   </i></font></a>      b_X[i0] = 0.0F;
<a name="651"><font color="#888888 "><i>  651   </i></font></a>    <b>}</b>
<a name="652"><font color="#888888 "><i>  652   </i></font></a>
<a name="653"><font color="#888888 "><i>  653   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 2; i0++) <b>{</b>
<a name="654"><font color="#888888 "><i>  654   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="655"><font color="#888888 "><i>  655   </i></font></a>        b_H[i0 + (i &lt;&lt; 1)] = 0.0F;
<a name="656"><font color="#888888 "><i>  656   </i></font></a>        <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 3; vcol++) <b>{</b>
<a name="657"><font color="#888888 "><i>  657   </i></font></a>          b_H[i0 + (i &lt;&lt; 1)] += H[i0 + (vcol &lt;&lt; 1)] * <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[vcol + 3 * i];
<a name="658"><font color="#888888 "><i>  658   </i></font></a>        <b>}</b>
<a name="659"><font color="#888888 "><i>  659   </i></font></a>      <b>}</b>
<a name="660"><font color="#888888 "><i>  660   </i></font></a>    <b>}</b>
<a name="661"><font color="#888888 "><i>  661   </i></font></a>
<a name="662"><font color="#888888 "><i>  662   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 2; i0++) <b>{</b>
<a name="663"><font color="#888888 "><i>  663   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="664"><font color="#888888 "><i>  664   </i></font></a>        tol = 0.0F;
<a name="665"><font color="#888888 "><i>  665   </i></font></a>        <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 3; vcol++) <b>{</b>
<a name="666"><font color="#888888 "><i>  666   </i></font></a>          tol += b_H[i0 + (vcol &lt;&lt; 1)] * H[i + (vcol &lt;&lt; 1)];
<a name="667"><font color="#888888 "><i>  667   </i></font></a>        <b>}</b>
<a name="668"><font color="#888888 "><i>  668   </i></font></a>
<a name="669"><font color="#888888 "><i>  669   </i></font></a>        c_H[i0 + (i &lt;&lt; 1)] = tol + R[i0 + (i &lt;&lt; 1)];
<a name="670"><font color="#888888 "><i>  670   </i></font></a>      <b>}</b>
<a name="671"><font color="#888888 "><i>  671   </i></font></a>    <b>}</b>
<a name="672"><font color="#888888 "><i>  672   </i></font></a>
<a name="673"><font color="#888888 "><i>  673   </i></font></a>    <a href="#fcn_eml_xgesvd" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_eml_xgesvd');" target="_self"><font color="#1122aa">eml_xgesvd</font></a>(c_H, U, s, V);
<a name="674"><font color="#888888 "><i>  674   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 4; i0++) <b>{</b>
<a name="675"><font color="#888888 "><i>  675   </i></font></a>      c_H[i0] = 0.0F;
<a name="676"><font color="#888888 "><i>  676   </i></font></a>    <b>}</b>
<a name="677"><font color="#888888 "><i>  677   </i></font></a>
<a name="678"><font color="#888888 "><i>  678   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="679"><font color="#888888 "><i>  679   </i></font></a>      c_H[i + (i &lt;&lt; 1)] = s[i];
<a name="680"><font color="#888888 "><i>  680   </i></font></a>    <b>}</b>
<a name="681"><font color="#888888 "><i>  681   </i></font></a>
<a name="682"><font color="#888888 "><i>  682   </i></font></a>    tol = 2.0F * c_H[0] * 1.1920929E-7F;
<a name="683"><font color="#888888 "><i>  683   </i></font></a>    r = 0;
<a name="684"><font color="#888888 "><i>  684   </i></font></a>    i = 0;
<a name="685"><font color="#888888 "><i>  685   </i></font></a>    <font color="#112266"><b>while</b></font> ((i + 1 &lt; 3) &amp;&amp; (c_H[i + (i &lt;&lt; 1)] &gt; tol)) <b>{</b>
<a name="686"><font color="#888888 "><i>  686   </i></font></a>      r++;
<a name="687"><font color="#888888 "><i>  687   </i></font></a>      i++;
<a name="688"><font color="#888888 "><i>  688   </i></font></a>    <b>}</b>
<a name="689"><font color="#888888 "><i>  689   </i></font></a>
<a name="690"><font color="#888888 "><i>  690   </i></font></a>    <font color="#112266"><b>if</b></font> (r &gt; 0) <b>{</b>
<a name="691"><font color="#888888 "><i>  691   </i></font></a>      vcol = 0;
<a name="692"><font color="#888888 "><i>  692   </i></font></a>      <font color="#112266"><b>for</b></font> (ar = 0; ar + 1 &lt;= r; ar++) <b>{</b>
<a name="693"><font color="#888888 "><i>  693   </i></font></a>        tol = 1.0F / c_H[ar + (ar &lt;&lt; 1)];
<a name="694"><font color="#888888 "><i>  694   </i></font></a>        <font color="#112266"><b>for</b></font> (i = vcol; i + 1 &lt;= vcol + 2; i++) <b>{</b>
<a name="695"><font color="#888888 "><i>  695   </i></font></a>          V[i] *= tol;
<a name="696"><font color="#888888 "><i>  696   </i></font></a>        <b>}</b>
<a name="697"><font color="#888888 "><i>  697   </i></font></a>
<a name="698"><font color="#888888 "><i>  698   </i></font></a>        vcol += 2;
<a name="699"><font color="#888888 "><i>  699   </i></font></a>      <b>}</b>
<a name="700"><font color="#888888 "><i>  700   </i></font></a>
<a name="701"><font color="#888888 "><i>  701   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i += 2) <b>{</b>
<a name="702"><font color="#888888 "><i>  702   </i></font></a>        <font color="#112266"><b>for</b></font> (ic = i; ic + 1 &lt;= i + 2; ic++) <b>{</b>
<a name="703"><font color="#888888 "><i>  703   </i></font></a>          b_X[ic] = 0.0F;
<a name="704"><font color="#888888 "><i>  704   </i></font></a>        <b>}</b>
<a name="705"><font color="#888888 "><i>  705   </i></font></a>      <b>}</b>
<a name="706"><font color="#888888 "><i>  706   </i></font></a>
<a name="707"><font color="#888888 "><i>  707   </i></font></a>      vcol = -1;
<a name="708"><font color="#888888 "><i>  708   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i += 2) <b>{</b>
<a name="709"><font color="#888888 "><i>  709   </i></font></a>        ar = 0;
<a name="710"><font color="#888888 "><i>  710   </i></font></a>        vcol++;
<a name="711"><font color="#888888 "><i>  711   </i></font></a>        i0 = (vcol + ((r - 1) &lt;&lt; 1)) + 1;
<a name="712"><font color="#888888 "><i>  712   </i></font></a>        <font color="#112266"><b>for</b></font> (ib = vcol; ib + 1 &lt;= i0; ib += 2) <b>{</b>
<a name="713"><font color="#888888 "><i>  713   </i></font></a>          <font color="#112266"><b>if</b></font> (U[ib] != 0.0F) <b>{</b>
<a name="714"><font color="#888888 "><i>  714   </i></font></a>            ia = ar;
<a name="715"><font color="#888888 "><i>  715   </i></font></a>            <font color="#112266"><b>for</b></font> (ic = i; ic + 1 &lt;= i + 2; ic++) <b>{</b>
<a name="716"><font color="#888888 "><i>  716   </i></font></a>              ia++;
<a name="717"><font color="#888888 "><i>  717   </i></font></a>              b_X[ic] += U[ib] * V[ia - 1];
<a name="718"><font color="#888888 "><i>  718   </i></font></a>            <b>}</b>
<a name="719"><font color="#888888 "><i>  719   </i></font></a>          <b>}</b>
<a name="720"><font color="#888888 "><i>  720   </i></font></a>
<a name="721"><font color="#888888 "><i>  721   </i></font></a>          ar += 2;
<a name="722"><font color="#888888 "><i>  722   </i></font></a>        <b>}</b>
<a name="723"><font color="#888888 "><i>  723   </i></font></a>      <b>}</b>
<a name="724"><font color="#888888 "><i>  724   </i></font></a>    <b>}</b>
<a name="725"><font color="#888888 "><i>  725   </i></font></a>
<a name="726"><font color="#888888 "><i>  726   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="727"><font color="#888888 "><i>  727   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="728"><font color="#888888 "><i>  728   </i></font></a>        b_K[i0 + 3 * i] = 0.0F;
<a name="729"><font color="#888888 "><i>  729   </i></font></a>        <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 2; vcol++) <b>{</b>
<a name="730"><font color="#888888 "><i>  730   </i></font></a>          b_K[i0 + 3 * i] += y[i0 + 3 * vcol] * b_X[vcol + (i &lt;&lt; 1)];
<a name="731"><font color="#888888 "><i>  731   </i></font></a>        <b>}</b>
<a name="732"><font color="#888888 "><i>  732   </i></font></a>      <b>}</b>
<a name="733"><font color="#888888 "><i>  733   </i></font></a>    <b>}</b>
<a name="734"><font color="#888888 "><i>  734   </i></font></a>
<a name="735"><font color="#888888 "><i>  735   </i></font></a>    <font color="#117755"><i>/* 增益 */</i></font>
<a name="736"><font color="#888888 "><i>  736   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 2; i0++) <b>{</b>
<a name="737"><font color="#888888 "><i>  737   </i></font></a>      tol = 0.0F;
<a name="738"><font color="#888888 "><i>  738   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="739"><font color="#888888 "><i>  739   </i></font></a>        tol += H[i0 + (i &lt;&lt; 1)] * <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i];
<a name="740"><font color="#888888 "><i>  740   </i></font></a>      <b>}</b>
<a name="741"><font color="#888888 "><i>  741   </i></font></a>
<a name="742"><font color="#888888 "><i>  742   </i></font></a>      s[i0] = Z[i0] - tol;
<a name="743"><font color="#888888 "><i>  743   </i></font></a>    <b>}</b>
<a name="744"><font color="#888888 "><i>  744   </i></font></a>
<a name="745"><font color="#888888 "><i>  745   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="746"><font color="#888888 "><i>  746   </i></font></a>      tol = 0.0F;
<a name="747"><font color="#888888 "><i>  747   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 2; i++) <b>{</b>
<a name="748"><font color="#888888 "><i>  748   </i></font></a>        tol += b_K[i0 + 3 * i] * s[i];
<a name="749"><font color="#888888 "><i>  749   </i></font></a>      <b>}</b>
<a name="750"><font color="#888888 "><i>  750   </i></font></a>
<a name="751"><font color="#888888 "><i>  751   </i></font></a>      <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i0] += tol;
<a name="752"><font color="#888888 "><i>  752   </i></font></a>    <b>}</b>
<a name="753"><font color="#888888 "><i>  753   </i></font></a>
<a name="754"><font color="#888888 "><i>  754   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 9; i0++) <b>{</b>
<a name="755"><font color="#888888 "><i>  755   </i></font></a>      b_F[i0] = <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0];
<a name="756"><font color="#888888 "><i>  756   </i></font></a>    <b>}</b>
<a name="757"><font color="#888888 "><i>  757   </i></font></a>
<a name="758"><font color="#888888 "><i>  758   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="759"><font color="#888888 "><i>  759   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="760"><font color="#888888 "><i>  760   </i></font></a>        tol = 0.0F;
<a name="761"><font color="#888888 "><i>  761   </i></font></a>        <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 2; vcol++) <b>{</b>
<a name="762"><font color="#888888 "><i>  762   </i></font></a>          tol += b_K[i0 + 3 * vcol] * H[vcol + (i &lt;&lt; 1)];
<a name="763"><font color="#888888 "><i>  763   </i></font></a>        <b>}</b>
<a name="764"><font color="#888888 "><i>  764   </i></font></a>
<a name="765"><font color="#888888 "><i>  765   </i></font></a>        b_I[i0 + 3 * i] = I[i0 + 3 * i] - tol;
<a name="766"><font color="#888888 "><i>  766   </i></font></a>      <b>}</b>
<a name="767"><font color="#888888 "><i>  767   </i></font></a>    <b>}</b>
<a name="768"><font color="#888888 "><i>  768   </i></font></a>
<a name="769"><font color="#888888 "><i>  769   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="770"><font color="#888888 "><i>  770   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="771"><font color="#888888 "><i>  771   </i></font></a>        <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0 + 3 * i] = 0.0F;
<a name="772"><font color="#888888 "><i>  772   </i></font></a>        <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 3; vcol++) <b>{</b>
<a name="773"><font color="#888888 "><i>  773   </i></font></a>          <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0 + 3 * i] += b_I[i0 + 3 * vcol] * b_F[vcol + 3 * i];
<a name="774"><font color="#888888 "><i>  774   </i></font></a>        <b>}</b>
<a name="775"><font color="#888888 "><i>  775   </i></font></a>      <b>}</b>
<a name="776"><font color="#888888 "><i>  776   </i></font></a>    <b>}</b>
<a name="777"><font color="#888888 "><i>  777   </i></font></a>  <b>}</b>
<a name="778"><font color="#888888 "><i>  778   </i></font></a>
<a name="779"><font color="#888888 "><i>  779   </i></font></a>  <font color="#112266"><b>if</b></font> (up == 0) <b>{</b>
<a name="780"><font color="#888888 "><i>  780   </i></font></a>    c_X = 0.0F;
<a name="781"><font color="#888888 "><i>  781   </i></font></a>    tol = 0.0F;
<a name="782"><font color="#888888 "><i>  782   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="783"><font color="#888888 "><i>  783   </i></font></a>      K[i0] = 0.0F;
<a name="784"><font color="#888888 "><i>  784   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="785"><font color="#888888 "><i>  785   </i></font></a>        K[i0] += <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0 + 3 * i] * H[1 + (i &lt;&lt; 1)];
<a name="786"><font color="#888888 "><i>  786   </i></font></a>      <b>}</b>
<a name="787"><font color="#888888 "><i>  787   </i></font></a>
<a name="788"><font color="#888888 "><i>  788   </i></font></a>      d_H[i0] = 0.0F;
<a name="789"><font color="#888888 "><i>  789   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="790"><font color="#888888 "><i>  790   </i></font></a>        d_H[i0] += H[1 + (i &lt;&lt; 1)] * <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i + 3 * i0];
<a name="791"><font color="#888888 "><i>  791   </i></font></a>      <b>}</b>
<a name="792"><font color="#888888 "><i>  792   </i></font></a>
<a name="793"><font color="#888888 "><i>  793   </i></font></a>      tol += d_H[i0] * H[1 + (i0 &lt;&lt; 1)];
<a name="794"><font color="#888888 "><i>  794   </i></font></a>    <b>}</b>
<a name="795"><font color="#888888 "><i>  795   </i></font></a>
<a name="796"><font color="#888888 "><i>  796   </i></font></a>    <a href="svd_c.html#fcn_svd" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_svd');" target="_self"><font color="#1122aa">svd</font></a>(tol + R[3], &amp;b_U, &amp;S, &amp;b_V);
<a name="797"><font color="#888888 "><i>  797   </i></font></a>    r = 0;
<a name="798"><font color="#888888 "><i>  798   </i></font></a>    <font color="#112266"><b>if</b></font> (!(S &gt; S * 1.1920929E-7F)) <b>{</b>
<a name="799"><font color="#888888 "><i>  799   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="800"><font color="#888888 "><i>  800   </i></font></a>      r = 1;
<a name="801"><font color="#888888 "><i>  801   </i></font></a>    <b>}</b>
<a name="802"><font color="#888888 "><i>  802   </i></font></a>
<a name="803"><font color="#888888 "><i>  803   </i></font></a>    <font color="#112266"><b>if</b></font> (r &gt; 0) <b>{</b>
<a name="804"><font color="#888888 "><i>  804   </i></font></a>      tol = 1.0F / S;
<a name="805"><font color="#888888 "><i>  805   </i></font></a>      i = 1;
<a name="806"><font color="#888888 "><i>  806   </i></font></a>      <font color="#112266"><b>while</b></font> (i &lt;= 1) <b>{</b>
<a name="807"><font color="#888888 "><i>  807   </i></font></a>        b_V *= tol;
<a name="808"><font color="#888888 "><i>  808   </i></font></a>        i = 2;
<a name="809"><font color="#888888 "><i>  809   </i></font></a>      <b>}</b>
<a name="810"><font color="#888888 "><i>  810   </i></font></a>
<a name="811"><font color="#888888 "><i>  811   </i></font></a>      <font color="#112266"><b>if</b></font> (b_U != 0.0F) <b>{</b>
<a name="812"><font color="#888888 "><i>  812   </i></font></a>        c_X = b_U * b_V;
<a name="813"><font color="#888888 "><i>  813   </i></font></a>      <b>}</b>
<a name="814"><font color="#888888 "><i>  814   </i></font></a>    <b>}</b>
<a name="815"><font color="#888888 "><i>  815   </i></font></a>
<a name="816"><font color="#888888 "><i>  816   </i></font></a>    <font color="#117755"><i>/* 增益 */</i></font>
<a name="817"><font color="#888888 "><i>  817   </i></font></a>    tol = 0.0F;
<a name="818"><font color="#888888 "><i>  818   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="819"><font color="#888888 "><i>  819   </i></font></a>      tol += H[1 + (i0 &lt;&lt; 1)] * <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i0];
<a name="820"><font color="#888888 "><i>  820   </i></font></a>      K[i0] *= c_X;
<a name="821"><font color="#888888 "><i>  821   </i></font></a>    <b>}</b>
<a name="822"><font color="#888888 "><i>  822   </i></font></a>
<a name="823"><font color="#888888 "><i>  823   </i></font></a>    tol = Z[1] - tol;
<a name="824"><font color="#888888 "><i>  824   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="825"><font color="#888888 "><i>  825   </i></font></a>      <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i0] += K[i0] * tol;
<a name="826"><font color="#888888 "><i>  826   </i></font></a>    <b>}</b>
<a name="827"><font color="#888888 "><i>  827   </i></font></a>
<a name="828"><font color="#888888 "><i>  828   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 9; i0++) <b>{</b>
<a name="829"><font color="#888888 "><i>  829   </i></font></a>      b_F[i0] = <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0];
<a name="830"><font color="#888888 "><i>  830   </i></font></a>    <b>}</b>
<a name="831"><font color="#888888 "><i>  831   </i></font></a>
<a name="832"><font color="#888888 "><i>  832   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="833"><font color="#888888 "><i>  833   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="834"><font color="#888888 "><i>  834   </i></font></a>        b_I[i0 + 3 * i] = I[i0 + 3 * i] - K[i0] * H[1 + (i &lt;&lt; 1)];
<a name="835"><font color="#888888 "><i>  835   </i></font></a>      <b>}</b>
<a name="836"><font color="#888888 "><i>  836   </i></font></a>    <b>}</b>
<a name="837"><font color="#888888 "><i>  837   </i></font></a>
<a name="838"><font color="#888888 "><i>  838   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="839"><font color="#888888 "><i>  839   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="840"><font color="#888888 "><i>  840   </i></font></a>        <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0 + 3 * i] = 0.0F;
<a name="841"><font color="#888888 "><i>  841   </i></font></a>        <font color="#112266"><b>for</b></font> (vcol = 0; vcol &lt; 3; vcol++) <b>{</b>
<a name="842"><font color="#888888 "><i>  842   </i></font></a>          <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i0 + 3 * i] += b_I[i0 + 3 * vcol] * b_F[vcol + 3 * i];
<a name="843"><font color="#888888 "><i>  843   </i></font></a>        <b>}</b>
<a name="844"><font color="#888888 "><i>  844   </i></font></a>      <b>}</b>
<a name="845"><font color="#888888 "><i>  845   </i></font></a>    <b>}</b>
<a name="846"><font color="#888888 "><i>  846   </i></font></a>  <b>}</b>
<a name="847"><font color="#888888 "><i>  847   </i></font></a>
<a name="848"><font color="#888888 "><i>  848   </i></font></a>  *h = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[0];
<a name="849"><font color="#888888 "><i>  849   </i></font></a>  *v = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[1];
<a name="850"><font color="#888888 "><i>  850   </i></font></a>  *a = <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[2];
<a name="851"><font color="#888888 "><i>  851   </i></font></a><b>}</b>
<a name="852"><font color="#888888 "><i>  852   </i></font></a>
<a name="853"><font color="#888888 "><i>  853   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="854"><font color="#888888 "><i>  854   </i></font></a><font color="#117755"><I> * Arguments    : void</i></font>
<a name="855"><font color="#888888 "><i>  855   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="856"><font color="#888888 "><i>  856   </i></font></a><font color="#117755"><I> */</i></font>
<a name="857"><font color="#888888 "><i>  857   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_nav_h_ekf_init">nav_h_ekf_init</a>(<font color="#112266"><b>void</b></font>)
<a name="858"><font color="#888888 "><i>  858   </i></font></a><b>{</b>
<a name="859"><font color="#888888 "><i>  859   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="860"><font color="#888888 "><i>  860   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> iv0[9] = <b>{</b> 100, 0, 0, 0, 100, 0, 0, 0, 100 <b>}</b>;
<a name="861"><font color="#888888 "><i>  861   </i></font></a>
<a name="862"><font color="#888888 "><i>  862   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 9; i++) <b>{</b>
<a name="863"><font color="#888888 "><i>  863   </i></font></a>    <a href="#var_P" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P');" target="_self"><font color="#1122aa">P</font></a>[i] = iv0[i];
<a name="864"><font color="#888888 "><i>  864   </i></font></a>  <b>}</b>
<a name="865"><font color="#888888 "><i>  865   </i></font></a>
<a name="866"><font color="#888888 "><i>  866   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="867"><font color="#888888 "><i>  867   </i></font></a>    <a href="#var_X" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_X');" target="_self"><font color="#1122aa">X</font></a>[i] = 0.0F;
<a name="868"><font color="#888888 "><i>  868   </i></font></a>  <b>}</b>
<a name="869"><font color="#888888 "><i>  869   </i></font></a><b>}</b>
<a name="870"><font color="#888888 "><i>  870   </i></font></a>
<a name="871"><font color="#888888 "><i>  871   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="872"><font color="#888888 "><i>  872   </i></font></a><font color="#117755"><I> * File trailer for nav_h_ekf.c</i></font>
<a name="873"><font color="#888888 "><i>  873   </i></font></a><font color="#117755"><I> *</i></font>
<a name="874"><font color="#888888 "><i>  874   </i></font></a><font color="#117755"><I> * [EOF]</i></font>
<a name="875"><font color="#888888 "><i>  875   </i></font></a><font color="#117755"><I> */</i></font>
<a name="876"><font color="#888888 "><i>  876   </i></font></a></pre>
</td></tr></table>
</p>
</body>
</html>